#include "KinematicChain.h"
#include "Utilities.h"
#include <iostream>
#include <fstream>
#include <string>
using namespace std;


template<> KinematicChain* Singleton<KinematicChain>::ms_Singleton = 0;


KinematicChain::KinematicChain() : Singleton(), Renderable() 
{
	for(int i = 0; i < DEG_FR; i++)
	{
		_a[i] = 0;
	}
	_selectedJoint == -1;
	InteractiveMode = true;
}


float &KinematicChain::A(int i)
{
	return _a[i];
}


mtxlib::vector3 &KinematicChain::V(int i)
{
	return _v[i];
}


mtxlib::vector3 &KinematicChain::U(int i)
{
	return _u[i];
}


float &KinematicChain::W(int i)
{
	return _w[i];
}


KinematicChain::JointType &KinematicChain::JT(int i)
{
	return _jointTypes[i];
}


mtxlib::matrix44 KinematicChain::TRP(int i) const
{
	mtxlib::matrix44 T = mtxlib::TranslateMatrix44(_v[i].x, _v[i].y, _v[i].z);
	mtxlib::matrix44 R = mtxlib::RotateRadMatrix44(_u[i], _w[i]);
	mtxlib::matrix44 P = mtxlib::IdentityMatrix44();
	if(_jointTypes[i] == JointType::Prismatic)
	{
		P = mtxlib::TranslateMatrix44(0, 0, _a[i]);
	}
	else
	{
		P = mtxlib::RotateRadMatrix44('z', _a[i]);
	}
	return T*R*P;
}


mtxlib::matrix44 KinematicChain::TRP() const
{
	mtxlib::matrix44 ret_val = mtxlib::IdentityMatrix44();
	for(int i = 0; i < DEG_FR; i++)
	{
		ret_val = ret_val * TRP(i);
	}
	return ret_val;
}


mtxlib::matrix44 KinematicChain::TR(int i) const
{
	mtxlib::matrix44 T = mtxlib::TranslateMatrix44(_v[i].x, _v[i].y, _v[i].z);
	mtxlib::matrix44 R = mtxlib::RotateRadMatrix44(_u[i], _w[i]);

	return T*R;
}


mtxlib::matrix44 KinematicChain::TR() const
{
	mtxlib::matrix44 ret_val = mtxlib::IdentityMatrix44();
	for(int i = 0; i < DEG_FR; i++)
	{
		ret_val = ret_val * TR(i);
	}
	return ret_val;
}


mtxlib::matrix44 KinematicChain::P(int i) const
{
	mtxlib::matrix44 P = mtxlib::IdentityMatrix44();
	if(_jointTypes[i] == JointType::Prismatic)
	{
		P = mtxlib::TranslateMatrix44(0, 0, _a[i]);
	}
	else
	{
		P = mtxlib::RotateRadMatrix44('z', _a[i]);
	}
	return P;
}


mtxlib::matrix44 KinematicChain::T(int i) const
{
	return mtxlib::TranslateMatrix44(_v[i].x, _v[i].y, _v[i].z);
}


mtxlib::matrix44 KinematicChain::R(int i) const
{
	return mtxlib::RotateRadMatrix44(_u[i], _w[i]);
}


mtxlib::matrix44 KinematicChain::R() const
{
	mtxlib::matrix44 ret_val = mtxlib::IdentityMatrix44();
	for(int i = 0; i < DEG_FR; i++)
	{
		ret_val = ret_val * R(i);
	}
	return ret_val;
}


mtxlib::matrix44 KinematicChain::RP(int i) const
{
	mtxlib::matrix44 R = mtxlib::RotateRadMatrix44(_u[i], _w[i]);
	mtxlib::matrix44 P = mtxlib::IdentityMatrix44();
	if(_jointTypes[i] == JointType::Prismatic)
	{
		P = mtxlib::TranslateMatrix44(0, 0, _a[i]);
	}
	else
	{
		P = mtxlib::RotateRadMatrix44('z', _a[i]);
	}
	return R*P;
}


mtxlib::matrix44 KinematicChain::RP() const
{
	mtxlib::matrix44 ret_val = mtxlib::IdentityMatrix44();
	for(int i = 0; i < DEG_FR; i++)
	{
		ret_val = ret_val * RP(i);
	}
	return ret_val;
}


void KinematicChain::Load(const char *path)
{
	string line;
	
	ifstream file(path);
	if (file.is_open())
	{
		for(int i = 0; i < DEG_FR; i++)
		{
			file>>_v[i].x>>_v[i].y>>_v[i].z;
			file>>_u[i].x>>_u[i].y>>_u[i].z;
			file>>_w[i];
			file>>_a[i];
			int jointType;
			file>>jointType;
			if(jointType == 0)
				_jointTypes[i] = JointType::Prismatic;
			else
				_jointTypes[i] = JointType::Rotational;

		}
		file.close();
	}
}


void KinematicChain::Save(const char *path) const
{
	ofstream file(path);
	if (file.is_open())
	{
		for(int i = 0; i < DEG_FR; i++)
		{
			file<<_v[i].x<<" "<<_v[i].y<<" "<<_v[i].z<<endl;
			file<<_u[i].x<<" "<<_u[i].y<<" "<<_u[i].z<<endl;
			file<<_w[i]<<endl;
			file<<_a[i]<<endl;
			file<<(_jointTypes[i] == JointType::Prismatic ? 0 : 1)<<endl;
		}
		file.close();
	}
}


void KinematicChain::ProcessSelection(int Color)
{
	if(!InteractiveMode)
	{
		return;
	}
	_selectedJoint = Color;
	IsSelected = true;
}


int &KinematicChain::SelectedJoint()
{
	return _selectedJoint;
}


void KinematicChain::GetRotationalJoints(int &first, int &second, int &third) const
{
	int i = 0;
	for(; i < DEG_FR; i++)
	{
		if(_jointTypes[i] == JointType::Rotational)
		{
			first = i;
			i++;
			break;
		}
	}
	for(; i < DEG_FR; i++)
	{
		if(_jointTypes[i] == JointType::Rotational)
		{
			second = i;
			i++;
			break;
		}
	}
	for(; i < DEG_FR; i++)
	{
		if(_jointTypes[i] == JointType::Rotational){

			third = i;
			i++;
			break;
		}
	}
}


void KinematicChain::GetPrismaticJoints(int &first, int &second, int &third) const
{
	int i = 0;
	for(; i < DEG_FR; i++)
	{
		if(_jointTypes[i] == JointType::Prismatic)
		{
			first = i;
			i++;
			break;
		}
	}
	for(; i < DEG_FR; i++)
	{
		if(_jointTypes[i] == JointType::Prismatic)
		{
			second = i;
			i++;
			break;
		}
	}
	for(; i < DEG_FR; i++)
	{
		if(_jointTypes[i] == JointType::Prismatic){

			third = i;
			i++;
			break;
		}
	}
}